from typing import Optional, List, Set, Tuple, Dict
import time
import random
import copy
import heapq
import math

NSD=0
WED=1

# 角落方向枚举 从左上角顺时针绕圈。
NW=0
NE=1
SE=2
SW=3

# 方向枚举
N=0
E=1
S=2
W=3

# 转向方向枚举
BACK=0
LEFT=1
STRAIGHT=2
RIGHT=3

# 坐标类
class Coordinates:
    def __init__(self, x, y):
        self.X = x
        self.Y = y
    def __add__(self, other: 'Coordinates') -> 'Coordinates':
        return Coordinates(self.X + other.X, self.Y + other.Y)
    def x(self):
        return self.X
    def y(self):
        return self.Y
    def setX(self, x):
        self.X = x
    def setY(self, y):
        self.Y = y
    # 放缩函数
    def amplify(self, times: float) -> 'Coordinates':
        return Coordinates((self.X * times), (self.Y * times))
    def distance(self, other: 'Coordinates') -> float:
        return ((self.X - other.X) ** 2 + (self.Y - other.Y) ** 2) ** 0.5
    def __str__(self):
        return "({},{})".format(self.X, self.Y)
    
# 矩形类
class Rectangle:
    def __init__(self, width: int, height: int):
        self.Width = width
        self.Height = height
    def height(self) -> int:
        return self.Height
    def width(self) -> int:
        return self.Width
    def setHeight(self, height: int):
        self.Height = height
    def setWidth(self, width: int):
        self.Hidth = width
    def amplify(self, times: float) -> 'Rectangle':
        return Rectangle(int(self.Width * times), int(self.Height * times))

# 路口节点坐标类
class NodeCoordinates:
    def __init__(self, coordinates: Coordinates, rectangle: Rectangle):
        """
        初始化路口节点坐标类

        Args:
            coordinates: 坐标
            height: 高度
            width: 宽度
        """
        self.coordinates = coordinates
        self.rectangle = rectangle

    # 返回节点的x坐标
    def x(self):
        return self.coordinates.x()
    # 返回节点的y坐标
    def y(self):
        return self.coordinates.y()
    def setX(self, x: int):
        self.coordinates.setX(x)
    def setY(self, y: int):
        self.coordinates.setY(y)
    def height(self) -> int:
        return self.rectangle.height()
    def width(self) -> int:
        return self.rectangle.width()
    def setHeight(self, height: int):
        self.rectangle.setHeight(height)
    def setWidth(self, width: int):
        self.rectangle.setWidth(width)
    # 路口节点放大指定倍数
    def amplify(self, times: float) -> 'NodeCoordinates':
        return NodeCoordinates(self.coordinates.amplify(times), self.rectangle.amplify(times))
    def getConners(self) -> List[Coordinates]:
        halfwidth = self.width()//2
        halfheight = self.height()//2
        x = self.x()
        y = self.y()
        # 从左上角顺时针绕圈返回四个角点坐标
        return [
            Coordinates(x-halfwidth,y-halfheight),
            Coordinates(x+halfwidth,y-halfheight),
            Coordinates(x+halfwidth,y+halfheight),
            Coordinates(x-halfwidth,y+halfheight),
                ]
    
    # 获得路口核心坐标
    def getCoreCoordinates(self) -> Coordinates:
        return copy.deepcopy(self.coordinates)

STOP=0
GO=1
RED = (255,0,0)
GREEN = (0,255,0)
# 交通信号灯类
class TrafficLight:
    def __init__(self, start_time, bias_time, init_state, last_time: Dict):
        """
        初始化交通信号灯

        Args:
            start_time: 开始时间
            bias_time: 偏移时间 60s
            init_state: 初始红绿灯状态
            last_time: 每个红绿灯状态持续时间，为一个字典，键为红绿灯状态，值为持续时间(s)
        """
        # self.start_time = start_time
        # self.bias_time = bias_time
        self.start_time = start_time-bias_time
        self.init_state = init_state
        self.state = init_state
        self.last_time = last_time
        self.map = {STOP: RED, GO: GREEN}
        self.show_number = 0
        self.biasx = None
        self.biasy = None
    
    # 更新交通信号灯状态
    def update_state(self):
        timestamp = time.time()
        lasttime = int(timestamp-self.start_time)
        cycletime = self.last_time[GO]+self.last_time[STOP]
        # 取余后得到一个小于周期的余数，用于计算红绿灯显示的数字和颜色
        yutime = lasttime % cycletime
        if yutime < self.last_time[self.init_state]:
            self.state = self.init_state
            self.show_number = int(self.last_time[self.init_state] - yutime)
        else:
            self.state = GO if self.init_state == STOP else STOP
            self.show_number = int(self.last_time[self.state]-(yutime-self.last_time[self.init_state]))
    
    # 获得交通信号灯状态
    def get_state(self):
        return self.state
    
    # 获得交通信号灯颜色
    def get_color(self):
        return self.map[self.state]
    
    def get_shownumber(self):
        return self.show_number
    
    def set_biasx(self, biasx):
        self.biasx = biasx
    
    def set_biasy(self, biasy):
        self.biasy = biasy

    def get_biasx(self):
        return self.biasx
    
    def get_biasy(self):
        return self.biasy

# 生成随机颜色
def random_color():
    r = random.randint(0,255)
    g = random.randint(0,255)
    b = random.randint(0,255)
    return (r,g,b)

# 汽车类
class Car:
    def __init__(self, coordinates: Coordinates, color: Tuple[int, int, int]):
        # 坐标
        self.coordinates = coordinates
        # 颜色
        self.color = color
        # 路径
        self.paths = []

    def set_x(self, x: int):
        self.coordinates.setX(x)
    def set_y(self, y: int):
        self.coordinates.setY(y)
    def get_x(self):
        return self.coordinates.x()
    def get_y(self):
        return self.coordinates.y()
    
    # 根据查询到的小车路径，将路径保存在车对象中。
    def add_paths(self, paths):
        self.paths = paths
        # TODO: Debug
        for node in self.paths:
            print(f"{node.name},",end="")
        print()
        self.paths_copy = copy.copy(paths)

    # TODO: Debug
    def print_paths_copy(self):
        print("path:",end="")
        for node in self.paths_copy:
            print(f"{node.name},",end="")
        print()
    
    # 删除当前离车最近的路口节点
    def pop0node(self):
        if len(self.paths) == 0:
            raise Exception("No path to pop")
        print(f"len = {len(self.paths)}")
        print(f"type = {type(self.paths)}")
        self.paths.pop(0)
    
    # 获得当前离车最近的路口节点
    def get0node(self):
        if len(self.paths) == 0:
            return None
        return self.paths[0]
    
    # 添加当前离车最近的路口节点
    def push0node(self, node):
        self.paths.insert(0, node)
    
    # 根据输入的方向和距离移动车辆
    def move_straight(self, direction: int, mov_dis: float, min_dis: float = 0.0, lastcar: 'Car' = None):
        """
        根据输入的方向和距离移动车辆

        Args:
            direction: 方向
            mov_dis: 移动距离
            min_dis: 两辆车的最小间距
            lastcar: 上一辆车
        """
        assert direction in [N,E,S,W], "direction should be N,E,S,W"
        selfx = self.coordinates.x()
        selfy = self.coordinates.y()
        if lastcar is None:
            if direction == N:
                self.set_y(selfy-mov_dis)
            elif direction == E:
                self.set_x(selfx+mov_dis)
            elif direction == S:
                self.set_y(selfy+mov_dis)
            elif direction == W:
                self.set_x(selfx-mov_dis)
            return
        lastx = lastcar.get_x()
        lasty = lastcar.get_y()
        if direction == N:
            newy = max(selfy-mov_dis, lasty+min_dis)
            self.set_y(newy)
        elif direction == E:
            newx = min(selfx+mov_dis, lastx-min_dis)
            self.set_x(newx)
        elif direction == S:
            newy = min(selfy+mov_dis, lasty-min_dis)
            self.set_y(newy)
        elif direction == W:
            newx = max(selfx-mov_dis, lastx+min_dis)
            self.set_x(newx)

    # 车辆只想移动到某个路的边界处
    def move_straight2bound(self, direction: int, mov_dis: float, NWnode: Coordinates, SEnode: Coordinates):
        """
        车辆只想移动到某个路的边界处，红灯或者其他必须在路口等待时，会调用此函数。

        Args:
            direction: 车辆移动方向
            mov_dis: 移动距离（单位：像素）
            NWnode: 北西方向的路口节点
            SEnode: 南东方向的路口节点
        """
        assert direction in [N,E,S,W], "direction should be N,E,S,W"
        x = self.get_x()
        y = self.get_y()
        x1 = NWnode.x()
        y1 = NWnode.y()
        x2 = SEnode.x()
        y2 = SEnode.y()
        # 获得边框（最小x，最大x，最小y，最大y）
        minx = min(x1,x2)
        maxx = max(x1,x2)
        miny = min(y1,y2)
        maxy = max(y1,y2)
        if direction == N:
            y = max(y-mov_dis, miny)
        elif direction == E:
            x = min(x+mov_dis, maxx)
        elif direction == S:
            y = min(y+mov_dis, maxy)
        elif direction == W:
            x = max(x-mov_dis, minx)
        self.set_x(x)
        self.set_y(y)

    # 车辆右转
    def turn_right(self, centerCoo: Coordinates, mov_ang: float, min_ang: float, lastcar: 'Car', radius: float):
        """
        车辆右转
        Args:
            centerCoo: 车辆的中心坐标
            mov_ang: 车辆右转的角度
            min_ang: 两辆车的最小角度
            lastcar: 上一辆车
        """
        # 根据点的坐标和圆心坐标计算得到角度
        def get_theta(x, y, centerx, centery):
            x1 = x-centerx
            y1 = y-centery
            theta = math.atan2(y1, x1)
            return theta
        
        x = self.get_x()
        y = self.get_y()
        old_theta = get_theta(x, y, centerCoo.x(), centerCoo.y())
        # 尝试获得当前车辆下一个时刻的角度，从而根据角度计算得到位置。
        if lastcar is None:
            new_theta = old_theta + mov_ang
        else:
            lastx = lastcar.get_x()
            lasty = lastcar.get_y()
            last_theta = get_theta(lastx, lasty, centerCoo.x(), centerCoo.y())
            if last_theta < old_theta:
                last_theta += 2*math.pi
            new_theta = min(last_theta-min_ang, old_theta+mov_ang)
        # 如果
        if new_theta > 2*math.pi:
            new_theta -= 2*math.pi
        x = centerCoo.x() + radius*math.cos(new_theta)
        y = centerCoo.y() + radius*math.sin(new_theta)
        self.set_x(x)
        self.set_y(y)

    def add_start_node(self, start_node):
        self.start_node = start_node

# 路口类
class Node:
    def __init__(self, rel_coordinates: NodeCoordinates, genCarProbability: float, name: str, father: 'Node' = None):
        """
        初始化路口节点

        Args:
            rel_coordinates: 相对坐标
            genCarfreq: 生成车的概率
            name: 节点名称
            father: 父节点
        """
        self.name = name
        # 节点相对坐标的父亲节点
        self.father = father
        # 生成汽车的概率
        self.genCarProbability = genCarProbability
        # 交通信号灯
        self.trafficlights: List[TrafficLight] = [None, None]
        # 如果没有父节点，那么这个节点就是根节点，传进来的相对坐标就是绝对坐标
        if father is None:
            self.relVirCoordinates = None
            self.absVirCoordinates = rel_coordinates
        # 如果有父节点，那么正常赋值
        else:
            self.relVirCoordinates = rel_coordinates
            self.absVirCoordinates = None
        # 画布坐标和画布尺寸
        self.absRelCoordinates = None
        self.offsetupdate = False
        self.paths = [None, None, None, None]
        self.nodes = [None, None, None, None]
        # 东南西北四个方向直行的车道，里面放置汽车
        self.straight_car_queues: List[List[Car]] = [[],[],[],[]]
        self.straight_car_queues_updated: List[bool] = [False, False, False, False]
        # 四个方向右转的车道，里面放置汽车
        self.right_car_queues: List[List[Car]] = [[],[],[],[]]
        self.right_car_queues_updated: List[bool] = [False, False, False, False]
        # 四个方向掉头的车道，里面放置汽车
        self.back_car_queues: List[List[Car]] = [None, None, None, None]
        self.back_car_queues_updated: List[bool] = [False, False, False, False]
        # 路口所有的方向的车道都更新后，这个置为True
        self.all_updated = False

    def get_path(self, direction: int):
        return self.paths[direction]

    # 每次所有的路口和道路更新后都清零更新标志
    def clean_all_update(self):
        self.straight_car_queues_updated = [False, False, False, False]
        self.right_car_queues_updated = [False, False, False, False]
        self.all_updated = False

    # 如果节点里所有车辆的位置都更新了，则返回True
    def get_all_updated(self):
        all_updated = True
        for s in self.straight_car_queues_updated:
            all_updated = all_updated and s
        for r in self.right_car_queues_updated:
            all_updated = all_updated and r
        for direction,b in enumerate(self.back_car_queues_updated):
            if direction in [W,E]:
                all_updated = all_updated and b
        return all_updated
    
    # 获得某个直行方向车道的最后一辆车
    def get_straight_end_car(self, from_direction: int):
        if len(self.straight_car_queues[from_direction]) == 0:
            return None
        return self.straight_car_queues[from_direction][-1]
    # 获得某个直行车道是否更新车辆位置 的状态
    def get_straight_car_queue_updated(self, from_direction: int) -> bool:
        return self.straight_car_queues_updated[from_direction]
    # 获得某个直行车道的车辆数量
    def get_straight_car_queue_length(self, from_direction: int) -> int:
        return len(self.straight_car_queues[from_direction])
    
    # 获得某个右转方向车道的最后一辆车
    def get_right_end_car(self, from_direction: int):
        if len(self.right_car_queues[from_direction]) == 0:
            return None
        return self.right_car_queues[from_direction][-1]
    # 获得某个右转车道是否更新车辆位置 的状态
    def get_right_car_queue_updated(self, from_direction: int):
        return self.right_car_queues_updated[from_direction]
    
    # 获得某个掉头方向车道的最后一辆车
    def get_back_end_car(self, from_direction: int):
        back_queue = self.back_car_queues[from_direction]
        if back_queue is None or len(back_queue) == 0:
            return None
        return back_queue[-1]
    # 获得某个掉头车道是否更新车辆位置 的状态
    def get_back_car_queue_updated(self, from_direction: int):
        return self.back_car_queues_updated[from_direction]
    # 标记某个掉头车道是否更新车辆位置 的状态 为True
    def mark_back_car_queue_updated(self, from_direction: int):
        self.back_car_queues_updated[from_direction] = True

    # 获得相对于父亲节点的相对虚拟距离
    def getRelativeDistance(self):
        assert self.father is not None, "father should not be None"
        assert self.relVirCoordinates is not None, "relVirCoordinates should not be None"
        if self.relVirCoordinates.x() == 0:
            return abs(self.relVirCoordinates.y())
        elif self.relVirCoordinates.y() == 0:
            return abs(self.relVirCoordinates.x())
        else:
            raise ValueError("the relative coordinates should be either horizontal or vertical")

    # 获得绝对虚拟坐标
    def getAbsVirCoordinates(self) -> NodeCoordinates:
        if self.absVirCoordinates is None:
            # 获得父节点的绝对虚拟坐标
            fatherAbsVirCoordinates = self.father.getAbsVirCoordinates()
            if self.relVirCoordinates.x() == 0:
                # 竖着的相对位置
                y = int(fatherAbsVirCoordinates.y() + self.relVirCoordinates.y())
                if self.relVirCoordinates.y() > 0:
                    y += int((fatherAbsVirCoordinates.height() + self.relVirCoordinates.height())/2)
                else:
                    y -= int((fatherAbsVirCoordinates.height() + self.relVirCoordinates.height())/2)
                self.absVirCoordinates = NodeCoordinates(Coordinates(fatherAbsVirCoordinates.x(), y), self.relVirCoordinates)
            elif self.relVirCoordinates.y() == 0:
                # 横着的相对位置
                x = int(fatherAbsVirCoordinates.x() + self.relVirCoordinates.x())
                if self.relVirCoordinates.x() > 0:
                    x += int((fatherAbsVirCoordinates.width() + self.relVirCoordinates.width())/2)
                else:
                    x -= int((fatherAbsVirCoordinates.width() + self.relVirCoordinates.width())/2)
                self.absVirCoordinates = NodeCoordinates(Coordinates(x, fatherAbsVirCoordinates.y()), self.relVirCoordinates)
        return self.absVirCoordinates
    
    def getConnerCoordinates(self) -> List[Coordinates]:
        # 获得四个角点坐标
        if self.absRelCoordinates is None:
            raise ValueError("absRelCoordinates should not be None")
        return self.absRelCoordinates.getConners()

    # 返回路口节点的虚拟x坐标
    def vx(self):
        if self.absVirCoordinates is None:
            self.getAbsVirCoordinates()
        return self.absVirCoordinates.x()

    # 返回路口节点的虚拟y坐标
    def vy(self):
        if self.absVirCoordinates is None:
            self.getAbsVirCoordinates()
        return self.absVirCoordinates.y()

    # 返回路口节点的虚拟高度
    def vheight(self):
        if self.relVirCoordinates is not None:
            return self.relVirCoordinates.height()
        else:
            return self.absVirCoordinates.height()

    # 返回路口节点的虚拟宽度
    def vwidth(self):
        if self.relVirCoordinates is not None:
            return self.relVirCoordinates.width()
        else:
            return self.absVirCoordinates.width()

    # 返回路口节点的实际x坐标
    def rx(self):
        return self.absRelCoordinates.x()
    
    # 返回路口节点的实际y坐标
    def ry(self):
        return self.absRelCoordinates.y()
    
    # 返回路口节点的实际宽度
    def rwidth(self):
        return self.absRelCoordinates.width()
    
    # 返回路口节点的实际高度
    def rheight(self):
        return self.absRelCoordinates.height()
    
    # 表示路口节点的属性信息
    def __str__(self):
        if self.absRelCoordinates is None:
            return f"({self.vx():2d},{self.vy():2d})"
        return f"Node V=({self.vx():2d}, {self.vy():2d}, {self.vwidth():2d}, {self.vheight():2d}) R=({self.rx():3d},{self.ry():3d},{self.rwidth():3d},{self.rheight():3d})"

    def biasAbsVirCoordinates(self, bias: Coordinates):
        """
        偏移节点的绝对虚拟坐标，用于在map中平移路口节点

        Args:
            bias: 偏移量
        """
        if not self.offsetupdate:
            self.offsetupdate = True
            self.absVirCoordinates.coordinates += bias

    def setAbsRelCoordinates(self, times: float, drawBase: Coordinates):
        """
        根据虚拟坐标更新画框坐标，先将逻辑坐标扩大成画布坐标，再在画布基础上加上一个基础坐标。

        Args:
            times: 放大倍数
            drawBase: 画框基准坐标
        """
        if self.absRelCoordinates is None:
            self.absRelCoordinates = self.absVirCoordinates.amplify(times)
            self.absRelCoordinates.coordinates += drawBase

    def __hash__(self):
        return hash(self.relVirCoordinates)
    
    def add_path(self, direction: int, path):
        # 如果方向不存在，报错
        if direction not in [N,E,S,W]:
            raise ValueError("direction should be N,E,S,W")
        # 如果路径已经存在，报错
        assert self.paths[direction] == None, "the node has already has a path in this direction"
        # 记录路径
        self.paths[direction]=path
        # 记录节点
        node = path.get_another_node(self)
        self.nodes[direction]=node

    # 生成一个车辆并返回
    def genCar(self):
        random_number = random.random()
        # 生成的随机数必须在0-1之间
        assert random_number<1 and random_number>=0, "random number should be between 0 and 1"
        # 如果随机数小于生成车的概率，返回None
        if random_number > self.genCarProbability:
            return None,None
        # 如果没有路径，报错
        assert self.paths != [None, None, None, None], "the node has no path"
        color = random_color()
        # 遍历当前路口所有的道路。直到找到路径。
        for i,path in enumerate(self.paths):
            if path is None:
                continue
            # 找到路径了。
            carCoo = self.absRelCoordinates.getCoreCoordinates()
            biasX = self.absRelCoordinates.width()//4
            biasY = self.absRelCoordinates.height()//4
            # 开始根据路口方向设置车辆的初始坐标
            if i == N:
                carCoo.setX(carCoo.x()+biasX)
            elif i == S:
                carCoo.setX(carCoo.x()-biasX)
            elif i == E:
                carCoo.setY(carCoo.y()+biasY)
            elif i == W:
                carCoo.setY(carCoo.y()-biasY)
            # 生成车辆
            car = Car(carCoo, color)
            return car, i

    # 向路口添加交通信号灯
    def add_trafficlight(self, trafficlight: TrafficLight, direction: int):
        """
        添加交通信号灯

        Args:
            trafficlight: 交通信号灯
            direction: 信号灯控制路口的南北方向还是东西方向
        """
        if direction == NSD:
            trafficlight.set_biasy(self.absRelCoordinates.height()/2)
        elif direction == WED:
            trafficlight.set_biasx(self.absRelCoordinates.width()/2)
        else:
            raise ValueError("direction should be NSD or WED")
        self.trafficlights[direction] = trafficlight

    # 获得路口节点的交通信号灯
    def get_trafficlight(self, direction: int) -> TrafficLight:
        """
        获得路口节点的交通信号灯的状态

        Args:
            direction: 信号灯控制路口的南北方向还是东西方向
        """
        assert direction in [NSD, WED], "direction should be NSD or WED"
        # 如果没有交通信号灯，那么路口可以直接走。
        if self.trafficlights[direction] is None:
            return None
        # 如果有交通信号灯，则返回交通信号灯的状态。
        return self.trafficlights[direction]
    
    # 获得路口的交通信号灯的状态 -> GO, STOP
    def get_traffic_state(self, from_direction: int) -> int:
        if from_direction == N or from_direction == S:
            trafficlight = self.get_trafficlight(NSD)
        elif from_direction == E or from_direction == W:
            trafficlight = self.get_trafficlight(WED)
        else:
            raise ValueError("from_direction should be N,S,E,W")
        # 如果没有交通信号灯，那么路口可以直接走。
        if trafficlight is None:
            return GO
        # 否则返回交通信号灯状态。
        return trafficlight.get_state()
    
    def update_trafficlight(self):
        for trafficlight in self.trafficlights:
            if trafficlight is not None:
                trafficlight.update_state()
    
    # 通过路口来获得路口相对于路口节点的方向
    def get_direction_by_road(self, road):
        if road not in self.paths:
            return None
        return self.paths.index(road)
    
    # 通过邻居路口来获得邻居相对于路口节点的方向
    def get_direction_by_node(self, node):
        if node not in self.nodes:
            return None
        return self.nodes.index(node)

    # 获得一辆车在当前路口的转向情况
    def get_turn_direction(self, from_direction, next_node: 'Node') -> int:
        """
        获得一辆车在当前路口的转向情况
        Args:
            from_direction: 车的来源方向
            next_node: 车的下一个路口节点
        """
        next_direction = self.get_direction_by_node(next_node)
        turn_direction = (next_direction+4-from_direction)%4
        return turn_direction

    # 将左转转化成右转+掉头+直行情况。
    def switch_left2other_direction(self, from_direction: int, car: Car):
        if from_direction in [N,S]:
            # 南北方向左转，先右转再掉头
            medium_direction = (from_direction+RIGHT)%4
            turn_direction = RIGHT
        elif from_direction in [E,W]:
            # 东西方向，先直走再掉头
            medium_direction = (from_direction+STRAIGHT)%4
            turn_direction = STRAIGHT
        medium_node = self.nodes[medium_direction]
        car.push0node(self)
        car.push0node(medium_node)
        return turn_direction
    
    def set_car_position_default(self, car: Car, from_direction: int):
        """
        将刚刚进入直行车道的车辆的位置设置为默认值

        Args:
            car: 车辆
            from_direction: 车辆的来源方向
        """
        corners = self.getConnerCoordinates()
        # 获得路口的西北角坐标和东南角坐标
        NWnode = corners[NW]
        SEnode = corners[SE]
        xmin = NWnode.x()
        xmax = SEnode.x()
        ymin = NWnode.y()
        ymax = SEnode.y()
        corex = self.absRelCoordinates.x()
        corey = self.absRelCoordinates.y()
        width = self.absRelCoordinates.width()
        height = self.absRelCoordinates.height()
        
        assert from_direction in [N,S,E,W], "from_direction should be N,S,E,W"

        if from_direction == N:
            car.set_x(corex-width/4)
            car.set_y(ymin)
        elif from_direction == S:
            car.set_x(corex+width/4)
            car.set_y(ymax)
        elif from_direction == E:
            car.set_x(xmax)
            car.set_y(corey-height/4)
        elif from_direction == W:
            car.set_x(xmin)
            car.set_y(corey+height/4)
    
    # 向路口节点添加车辆。
    def add_car(self, car: Car, from_direction: int):
        """
        向路口添加车辆

        Args:
            car: 车辆
            direction: 车辆的来源方向
        """
        next_node = car.get0node()
        # 到边界路口了，直接抛弃车辆
        if next_node is None:
            return

        # 得到车辆转向方向
        assert self!=next_node, "the car should not go back to the same node"
        turn_direction = self.get_turn_direction(from_direction, next_node)
        # TODO: debug信息
        print(f"topnode:{next_node.name}, self:{self.name}")
        car.print_paths_copy()
        assert next_node != self, "topnode should not be self"
        # 把当前node从车辆栈中弹出
        car.pop0node()
        # 处理左转(特殊处理，将左转转化成x右转+掉头+直行)
        if turn_direction == LEFT:
            turn_direction = self.switch_left2other_direction(from_direction, car)
        
        if turn_direction == STRAIGHT:
            # 1. 将新的车加入直行车道
            self.straight_car_queues[from_direction].append(car)
            # 2. 将新的车的位置设为直行默认位置
            self.set_car_position_default(car, from_direction)
        elif turn_direction == RIGHT:
            # 1. 将新的车加入右转车道
            self.right_car_queues[from_direction].append(car)
            # 2. 将新的车的位置设为右转默认位置
            self.set_car_position_default(car, from_direction)
        elif turn_direction == BACK:
            if self.back_car_queues[from_direction] is None:
                self.back_car_queues[from_direction] = []
            # 1. 将新的车加入掉头车道
            self.back_car_queues[from_direction].append(car)
            # 2. 将新的车的位置设为掉头默认位置
            self.set_car_position_default(car, from_direction)
        else:
            raise ValueError("turn_direction should be STRAIGHT, RIGHT, LEFT or BACK")

    # 判断车辆是否在路口内部
    def is_in_node(self, coordinates: Coordinates) -> bool:
        coos = self.getConnerCoordinates()
        coo1 = coos[NW]
        coo2 = coos[SE]
        x1 = coo1.x()
        y1 = coo1.y()
        x2 = coo2.x()
        y2 = coo2.y()
        x = coordinates.x()
        y = coordinates.y()
        if (x-x1)*(x-x2)<=0 and (y-y1)*(y-y2)<=0:
            return True
        else:
            return False

    # 路口更新当前路口下所有车辆的位置。
    def update_straight_cars_position(self, mov_pix: float, min_dis: float, from_direction: int, last_car: Car):
        """
        更新直行车辆的位置

        Args:
            mov_pix: 移动像素
            min_dis: 最小距离
            lastcar: 上一个车辆
        """
        assert from_direction in [N,E,S,W], "from_direction should be N,E,S,W"
        to_direction = (from_direction+STRAIGHT)%4
        lastcar = last_car
        for car in self.straight_car_queues[from_direction]:
            car.move_straight(to_direction, mov_pix, min_dis, lastcar)
            lastcar = car
        # 2. 
        self.straight_car_queues_updated[from_direction] = True

        # 3.
        if len(self.straight_car_queues[from_direction]) == 0:
            return (None, None)
        if self.is_in_node(self.straight_car_queues[from_direction][0].coordinates):
            return (None, None)
        vehicle = self.straight_car_queues[from_direction].pop(0)
        return (vehicle, self.paths[to_direction])

    def update_right_cars_position(self, mov_ang: float, min_ang: float, from_direction: int, last_car: Car):
        """
        更新右转车辆的位置

        Args:
            mov_pix: 移动角度
            min_ang: 最小距离
            from_direction: 车辆的来源方向
            last_car: 上一个车辆的坐标
        """
        r = self.absRelCoordinates.height()/4
        # 获得路口的四个角的坐标
        corners = self.getConnerCoordinates()
        # 只有当角落方向枚举为从左上角开始顺时针绕圈时，才可以这样进行赋值
        corner_direction = from_direction
        thiscorner = corners[corner_direction]
        center_x = thiscorner.x()
        center_y = thiscorner.y()
        lastcar = last_car

        # 1. 从前向后遍历更新一个右转车道上面的每一辆车
        for car in self.right_car_queues[from_direction]:
            car.turn_right(Coordinates(center_x, center_y), mov_ang, min_ang, lastcar, r)
            lastcar = car
        
        # 2. 完成当前右转车道的所有车辆的位置更新记得标记一下
        self.right_car_queues_updated[from_direction] = True

        # 如果车最后没有出界，则返回None
        # 3. 如果车道没有车，那么返回None
        if len(self.right_car_queues[from_direction]) == 0:
            return (None,None)
        # 4. 如果车道有车，但是最后一辆车没有出界，则返回None
        if self.is_in_node(self.right_car_queues[from_direction][0].coordinates):
            return (None,None)
        # 5. 如果车道有车出界了。
        vehicle = self.right_car_queues[from_direction].pop(0)
        to_direction = (from_direction+RIGHT)%4
        to_road = self.paths[to_direction]
        return (vehicle, to_road)
        
    # 路口更新掉头车辆的位置    
    def update_back_cars_position(self, mov_pix: float, min_dis: float, from_direction: int):
        """
        更新掉头车辆的位置

        Args:
            mov_pix: 移动像素
            min_dis: 最小距离
        """
        assert from_direction in [N,E,S,W], "from direction should be N,E,S,W"
        # 移动方向
        to_direction = (from_direction+LEFT)%4
        lastcar = None
        self.back_car_queues_updated[from_direction] = True
        # 1. 从前向后更新掉头车道上面的每一辆车
        if self.back_car_queues[from_direction] is None:
            return (None, None)
        for car in self.back_car_queues[from_direction]:
            car.move_straight(to_direction, mov_pix/2, min_dis/2, lastcar)
            lastcar = car
        # 2. 将更新状态标志设置为True

        def out_of_back_queue(car: Car, from_direction: int, node: NodeCoordinates):
            """
            移动到掉头车道的外面
            """
            x = node.x()
            y = node.y()
            width = node.width()
            height = node.height()
            if from_direction == N:
                return car.get_x() >= x+width//4
            elif from_direction == E:
                return car.get_y() >= y+height//4
            elif from_direction == S:
                return car.get_x() <= x-width//4
            elif from_direction == W:
                return car.get_y() <= y-height//4
            else:
                raise ValueError("from_direction should be N,E,S,W")
        # 3. 如果车道没有车辆，就直接返回None
        if len(self.back_car_queues[from_direction]) == 0:
            return (None, None)
        # 4. 如果车道有车，但是最后一辆车没有出界，则返回None
        if not out_of_back_queue(self.back_car_queues[from_direction][0], from_direction, self.absRelCoordinates):
            return (None, None)
        # 5. 如果车道有车出界了。
        vehicle = self.back_car_queues[from_direction].pop(0)
        to_direction = (from_direction+BACK)%4
        to_road = self.paths[to_direction]
        return (vehicle, to_road)
        

class Line:
    def __init__(self, start_node: Coordinates, end_node: Coordinates):
        self.start_node = start_node
        self.end_node = end_node
        self.direction = None
        if self.start_node.x() == self.end_node.x():
            self.direction = NSD
        elif self.start_node.y() == self.end_node.y():
            self.direction = WED
        else:
            raise ValueError("Line should be vertical or horizontal")

# 路类 只能横平竖直
class Road:
    def __init__(self, nodelist: List[Node], name:str, draw_isolation_belt_width: Optional[float]=0.0):
        """
        初始化路类

        Args:
            nodelist: 路的节点列表，长度只能为2
            name: 路的名称
            draw_isolation_belt_width: 隔离带宽度占路面宽度的比例

        """
        self.name = name
        assert len(nodelist)==2, "nodelist should have at least 2 nodes"
        x0 = nodelist[0].vx()
        y0 = nodelist[0].vy()
        x1 = nodelist[1].vx()
        y1 = nodelist[1].vy()

        # 路的方向
        self.direction = None
        if x0 == x1:
            self.direction = NSD
        elif y0==y1:
            self.direction = WED
        else:
            raise ValueError("Road should be vertical or horizontal")

        """
        NSD=0
        WED=1

        N=0=NSD
        E=1=WED
        S=2=NSD+2
        W=3=WED+2
        """

        # 节点列表。虽然list中有四个，但是一个正常道路经过下面的计算后只剩下两个节点。
        # index为to_direction
        self.nodes: List[Node] = [None, None, None, None]
        if self.direction == NSD:
            if y0 < y1:
                self.nodes[N] = nodelist[0]
                self.nodes[S] = nodelist[1]
            else:
                self.nodes[N] = nodelist[1]
                self.nodes[S] = nodelist[0]
        elif self.direction == WED:
            if x0 < x1:
                self.nodes[W] = nodelist[0]
                self.nodes[E] = nodelist[1]
            else:
                self.nodes[W] = nodelist[1]
                self.nodes[E] = nodelist[0]

        # 路的两端的节点必须是等长或者等宽的
        if self.direction == NSD:
            assert self.nodes[N].vwidth() == self.nodes[S].vwidth(), "the width of two nodes should be equal"
        elif self.direction == WED:
            assert self.nodes[W].vheight() == self.nodes[E].vheight(), "the height of two nodes should be equal"

        # 四条车道，其中只有两条有效车道
        # 外层list的index为from_direction
        self.car_queues: List[List[Car]] = [None, None, None, None]
        self.car_queues_updated: List[bool] = [False, False, False, False]

        if self.direction == NSD:
            self.car_queues[N] = []
            self.car_queues[S] = []
        elif self.direction == WED:
            self.car_queues[W] = []
            self.car_queues[E] = []

        # 隔离带宽度（百分比）
        self.draw_isolation_belt_width = draw_isolation_belt_width

        # 路的虚拟长度
        self.virLen = int(0)
        
        # 向路的每一个路口添加道路
        for direction, node in enumerate(self.nodes):
            if node is None:
                continue
            node.add_path((direction+STRAIGHT)%4, self)

        # 画路的两条线坐标
        self.drawRoad: List[Line] = None

        # 画布上，路的四个角点的真实坐标，按照从左上角开始顺时针绕圈排列
        self.real_corners: List[Coordinates] = [None, None, None, None]

    # 获得车道是南北方向还是东西方向
    def get_direction(self):
        return self.direction
    
    # 如果道路上所有的车辆位置都更新了，则返回True
    def get_all_updated(self):
        if self.direction == NSD:
            li = [N,S]
        elif self.direction == WED:
            li = [W,E]
        else:
            raise ValueError("Road should be vertical or horizontal")
        re = True
        for i in li:
            re = re and self.car_queues_updated[i]
        return re
    
    # 标记所有车道的更新状态为False
    def clean_all_update(self):
        if self.direction == NSD:
            li = [N,S]
        elif self.direction == WED:
            li = [W,E]
        else:
            raise ValueError("Road should be vertical or horizontal")
        for i in li:
            self.car_queues_updated[i] = False

    # 获得路的长度
    def vlen(self) -> int:
        if self.virLen==0:
            start_coo = self.nodes[self.direction].getAbsVirCoordinates()
            end_coo = self.nodes[self.direction+2].getAbsVirCoordinates()
            if self.direction == NSD:
                self.virLen = abs(start_coo.y() - end_coo.y())-int((start_coo.height()+end_coo.height())/2)
            elif self.direction == WED:
                self.virLen = abs(start_coo.x() - end_coo.x())-int((start_coo.width()+end_coo.width())/2)
        return self.virLen
    
    # 根据方向，获得一个路线端点的路口
    def getNode(self, to_direction: int) -> Node:
        return self.nodes[to_direction]
    
    # 获得单向车道最前车
    def get_front_car(self, from_direction: int) -> Car:
        queue = self.car_queues[from_direction]
        if queue is None or len(queue) == 0:
            return None
        return queue[0]
    
    # 获得路的所有路口
    def getNodes(self) -> List[Node]:
        d = self.direction
        return [self.nodes[d], self.nodes[d+2]]

    # 生成画布上路的两条线
    def gen2Lines(self):
        if self.drawRoad is not None:
            return
        self.drawRoad = [None, None, None, None]
        # 如果路的方向是竖向
        if self.direction == NSD:
            # 获得两个路口中，每个路口的四个角点坐标
            big_conners = self.nodes[S].getConnerCoordinates()
            small_conners = self.nodes[N].getConnerCoordinates()
            # 左边的路线
            self.drawRoad[0] = Line(big_conners[NW], small_conners[SW])
            # 右边的路线
            self.drawRoad[1] = Line(big_conners[NE], small_conners[SE])

            # 记录路的四个角点真实坐标
            self.real_corners[SW] = big_conners[NW]
            self.real_corners[NW] = small_conners[SW]
            self.real_corners[SE] = big_conners[NE]
            self.real_corners[NE] = small_conners[SE]
        elif self.direction == WED:
            big_conners = self.nodes[E].getConnerCoordinates()
            small_conners = self.nodes[W].getConnerCoordinates()
            # 上面的路线
            self.drawRoad[0] = Line(big_conners[NW], small_conners[NE])
            # 下面的路线
            self.drawRoad[1] = Line(big_conners[SW], small_conners[SE])

            self.real_corners[NE] = big_conners[NW]
            self.real_corners[NW] = small_conners[NE]
            self.real_corners[SE] = big_conners[SW]
            self.real_corners[SW] = small_conners[SE]
        else:
            raise ValueError("Road should be vertical or horizontal")
    
    # 获得路的两条线
    def getLines(self) -> List[Line]:
        return self.drawRoad

    # 判断坐标是否在路上
    def isinRoad(self, coordinates: Coordinates) -> bool:
        coo1 = self.real_corners[NW]
        coo2 = self.real_corners[SE]
        x = coordinates.x()
        y = coordinates.y()
        x1,y1 = coo1.x(), coo1.y()
        x2,y2 = coo2.x(), coo2.y()
        if (x-x1)*(x-x2)<=0 and (y-y1)*(y-y2)<=0:
            return True
        else:
            print(f"({x},{y}):({x1},{y1})~({x2},{y2})")
            return False

    # 获得节点相对于路的方向
    def get_direction_by_node(self, node: Node) -> int:
        assert node in self.nodes, "node is not in road"
        return self.nodes.index(node)

    # 车道获得某个方向上车辆队列是否已经完成位置更新
    def get_car_queue_updated(self, from_direction: int):
        if self.direction == NSD:
            assert from_direction in [N,S], "from_direction should be N or S"
        elif self.direction == WED:
            assert from_direction in [W,E], "from_direction should be W or E"
        else:
            raise ValueError("Road should be vertical or horizontal")
        return self.car_queues_updated[from_direction]

    # 更新单个车道的车的位置
    def update_vehicles_bydirec(self, mov_pix: float, min_dis: float, from_direction: int, last_car: Car, traffic_stop: bool=False) -> Tuple[Optional[Car], Optional[Node]]:
        """
        更新单个车道的车辆位置

        Args:
            mov_pix: 移动像素
            min_dis: 和前车的最小距离
            from_direction: 车道来源方向
            traffic_stop: 是否遇到红灯停车

        Returns:
            (车辆, 目标节点)
        """
        # if ifstart2end:
        #     start_node = self.start_node
        #     end_node = self.end_node
        #     start2end = self.start2end
        # else:
        #     start_node = self.end_node
        #     end_node = self.start_node
        #     start2end = self.end2start

        if self.direction == NSD:
            assert from_direction in [N,S], "from_direction should be N or S"
        elif self.direction == WED:
            assert from_direction in [W,E], "from_direction should be W or E"
        else:
            raise ValueError("Road should be vertical or horizontal")
        
        start2end = self.car_queues[from_direction]
        to_direction = (from_direction+STRAIGHT)%4
        end_node = self.nodes[to_direction]
        # 前车
        lastcar: Car = last_car
        # 找到目标节点相对于路线的方向
        direction = self.get_direction_by_node(end_node)
        # 从头遍历每一辆车
        # 如果在路口直行遇到红灯了，需要停下来等待。
        if traffic_stop:
            for i, car in enumerate(start2end):
                # 那么头车就要行驶到路口再停下来。
                if i==0:
                    # 获得路的西北角坐标和东南角坐标（画布坐标）
                    NWnode = self.real_corners[NW]
                    SEnode = self.real_corners[SE]
                    car.move_straight2bound(direction, mov_pix, NWnode, SEnode)
                    lastcar = car
                # 其他车根据前车的位置调整位置即可
                else:
                    car.move_straight(direction, mov_pix, min_dis, lastcar)
                    lastcar = car
        else:
            for car in start2end:
                car.move_straight(direction, mov_pix, min_dis, lastcar)
                lastcar = car
        # 完成位置更新记得标记一下
        self.car_queues_updated[from_direction] = True
        # 如果车最后没有出界，则返回None 
        if len(start2end) == 0:
            return (None, None)
        if self.isinRoad(start2end[0].coordinates):
            return (None, None)
        # 如果车辆走出这条路了，把车辆和目标节点返回
        vehicle = start2end.pop(0)
        return (vehicle, end_node)

    # 从路口向道路上添加新的汽车
    def add_car(self, init_car: Car, min_dis: float, from_direction: int):
        assert from_direction in [N,E,S,W], "from_direction should be N,E,S,W"

        # 获得车的坐标
        x = init_car.get_x()
        y = init_car.get_y()

        # 判断车辆是否在路上
        if not self.isinRoad(init_car.coordinates):
            print(f"Warning: car {init_car.coordinates} is out of road")
            raise ValueError("Car is out of road")
        print(f"original car position: ({x},{y})")
        # 如果车道不存在，创建车道
        if self.car_queues[from_direction] is None:
            self.car_queues[from_direction] = []

        # 如果车道没有车，直接添加车
        if len(self.car_queues[from_direction]) == 0:
            self.car_queues[from_direction].append(init_car)
            return
        
        # 如果有前车，先保证和前车的最小距离
        last_car = self.car_queues[from_direction][-1]
        last_x = last_car.get_x()
        last_y = last_car.get_y()

        if from_direction == N:
            y = min(last_y-min_dis, y)
        elif from_direction == S:
            y = max(last_y+min_dis, y)
        elif from_direction == E:
            x = max(last_x-min_dis, x)
        elif from_direction == W:
            x = min(last_x+min_dis, x)
        else:
            raise ValueError("from_direction should be N,E,S,W")
        
        if not self.isinRoad(Coordinates(x, y)):
            return
            # raise ValueError("Car is out of road")
        
        init_car.set_x(x)
        init_car.set_y(y)
        self.car_queues[from_direction].append(init_car)

    # 获得另一个节点
    def get_another_node(self, node: Node) -> Node:
        assert node in self.nodes, "node is not in road"
        index=self.nodes.index(node)
        return self.nodes[(index+2)%4]
    
    # 判断一个车辆在路上经过移动后，会不会被移出道路
    def will_move_outof_bound(self, from_direction: int, mov_pix: float) -> bool:
        queue = self.car_queues[from_direction]
        if queue is None:
            return False
        if len(queue) == 0:
            return False
        front_car = queue[0]
        to_direction = (from_direction+STRAIGHT)%4
        front_car.move_straight(to_direction, mov_pix)
        coo = copy.deepcopy(front_car.coordinates)
        front_car.move_straight(from_direction, mov_pix)
        if not self.isinRoad(coo):
            return True
        return False
# 地图类
class Map:
    # 输出邻接表
    def print_graph(self):
        for k, v in self.graph.items():
            print(f"{k}:")
            for k1, v1 in v.items():
                print(f"    (to {k1}: distance={v1})")
            print()

    def print_graph_with_name(self):
        for k, v in self.graph.items():
            print(f"{k}:")
            for k1, v1 in v.items():
                print(f"    (to {k1}: distance={v1})")
            print()

    def dijkstra(self, graph, start):
        """
        Dijkstra 算法实现
        :param graph: 图的邻接表表示，例如：
                      {
                          'A': {'B': 1, 'C': 4},
                          'B': {'A': 1, 'C': 2, 'D': 5},
                          'C': {'A': 4, 'B': 2, 'D': 1},
                            'D': {'B': 5, 'C': 1}
                    }
        :param start: 起始节点
        :return: (distances, previous_nodes)，包含从起点到每个节点的最短距离和路径
        """
        # 初始化数据结构
        distances = {node: float('inf') for node in graph}  # 记录每个节点的最短距离
        # print(distances)
        distances[start] = 0  # 起点距离为 0
        previous_nodes = {node: None for node in graph}  # 记录路径上的前置节点
        priority_queue = [(0, start)]  # 优先队列，存储 (距离, 节点)

        while priority_queue:
            current_distance, current_node = heapq.heappop(priority_queue)  # 弹出最小距离的节点
        
            # 如果当前距离大于已记录的最小距离，则跳过
            if current_distance > distances[current_node]:
                continue

            # 遍历当前节点的邻居
            for neighbor, weight in graph[current_node].items():
                distance = current_distance + weight
            
                # 如果找到更短的路径，更新距离和路径
                if distance < distances[neighbor]:
                    distances[neighbor] = distance
                    previous_nodes[neighbor] = current_node
                    heapq.heappush(priority_queue, (distance, neighbor))  # 将更新后的距离和节点加入队列

        return distances, previous_nodes
    
    def reconstruct_path(self, previous_nodes, start, end):
        """
        重建从起点到目标节点的路径
        :param previous_nodes: 前置节点字典
        :param start: 起始节点
        :param end: 目标节点
        :return: 路径列表
        """
        path = []
        current = end
        while current is not None:
            path.append(current)
            current = previous_nodes[current]
        path.reverse()
        return path if path[0] == start else []  # 确保路径从起点开始
    
    def __init__(self, roads: List[Road], genCarNodes: Set[Node], trafficLightNodes: Set[Node]):
        self.roads = roads
        self.genCarNodes = genCarNodes
        self.trafficLightNodes = trafficLightNodes
        class tmpcoo:
            def __init__(self, x: int, node: Node):
                self.x = x
                self.node = node
        # 地图中，所有节点的虚拟坐标的最大x，最大y，最小x，最小y
        self.maxx = tmpcoo(0, None)
        self.minx = tmpcoo(0, None)
        self.maxy = tmpcoo(0, None)
        self.miny = tmpcoo(0, None)

        # 记录最大x，最大y，最小x，最小y的值和节点
        def updateOffset(maxx: tmpcoo, minx:tmpcoo, maxy:tmpcoo, miny:tmpcoo, node: Node):
            tmp_coordinates = node.getAbsVirCoordinates()
            if tmp_coordinates.x() > maxx.x:
                maxx.x = tmp_coordinates.x()
                maxx.node = node
            if tmp_coordinates.x() < minx.x:
                minx.x = tmp_coordinates.x()
                minx.node = node
            if tmp_coordinates.y() > maxy.x:
                maxy.x = tmp_coordinates.y()
                maxy.node = node
            if tmp_coordinates.y() < miny.x:
                miny.x = tmp_coordinates.y()
                miny.node = node
        # 获得整个地图中所有节点的最大x，最大y，最小x，最小y
        # for r in self.roads:
        #     updateOffset(self.maxx, self.minx, self.maxy, self.miny, r.getStartNode())
        #     updateOffset(self.maxx, self.minx, self.maxy, self.miny, r.getEndNode())
        for node in self.genCarNodes:
            updateOffset(self.maxx, self.minx, self.maxy, self.miny, node)
        
        # 根据上面获得的最小x和最小y，计算画布上所有的点的逻辑坐标需要偏移的坐标。
        self.virYOffset = -self.miny.x
        self.virXOffset = -self.minx.x
        # 从逻辑坐标到画布坐标的放缩倍数
        self.realTimes = None
        # 记录一下画布的尺寸
        self.drawWindow: Rectangle = None
        # 邻接表
        self.graph = {}
        def addGraphPath(graph, node: Node):
            if node.father is None:
                return
            if node not in graph.keys():
                graph[node] = {}
            if node.father not in graph.keys():
                graph[node.father] = {}
            distance = node.getRelativeDistance()
            # graph[node][node.father] = [[node, node.father], distance]
            # graph[node.father][node] = [[node.father, node], distance]
            graph[node][node.father] = distance
            graph[node.father][node] = distance
        
        roadnum = len(self.roads)
        print(f"roadnum: {roadnum}")
        # 所有路口组成的集合
        self.nodelist = set()
        for r in self.roads:
            # start_node = r.getStartNode()
            # end_node = r.getEndNode()
            # # 把所有的路都放到邻接表中
            # addGraphPath(self.graph, start_node)
            # addGraphPath(self.graph, end_node)
            # # 路口加入集合中
            # self.nodelist.add(start_node)
            # self.nodelist.add(end_node)

            nodes = r.getNodes()
            for node in nodes:
                # 把所有的路都放到邻接表中
                addGraphPath(self.graph, node)
                # 路口加入集合中
                self.nodelist.add(node)
        
        # 打印邻接表
        self.print_graph()
        # 计算所有节点之间的最短路径
        self.paths = {}
        for start_node in self.nodelist:
            self.paths[start_node] = {}
            distances, previous_nodes = self.dijkstra(self.graph, start_node)
            for end_node in self.nodelist:
                path = self.reconstruct_path(previous_nodes, start_node,end_node)
                self.paths[start_node][end_node] = path
        # 输出所有节点之间的路径（中间节点）
        # for k, v in self.paths.items():
        #     print(f"{k}:")
        #     for k1, v1 in v.items():
        #         print(f"    (to {k1}: path=",end="")
        #         for node in v1:
        #             print(f"{node},", end="")
        #         print(f")")
        #     print()
        
        for k, v in self.paths.items():
            print(f"{k.name}:")
            for k1, v1 in v.items():
                # print(f"    (to {k1}: path=",end="")
                print("    ",end="")
                for node in v1:
                    print(f"{node.name},", end="")
                print("")
            print()
        

    # 获得地图实际放缩倍数
    def getRealTimes(self, window: Rectangle) -> int:
        if self.realTimes is None:
            self.drawWindow = window
            useful_width = int(window.width()*0.9)
            useful_height = int(window.height()*0.9)
            widTimes = useful_width // (self.maxx.x - self.minx.x)
            # print(f"widTimes = {useful_width} // ( {self.maxx.x} - {self.minx.x} )")
            heiTimes = useful_height // (self.maxy.x - self.miny.x)
            # print(f"widTimes: {widTimes}, heiTimes: {heiTimes}")
            self.realTimes = min(widTimes, heiTimes)
        return self.realTimes
    
    # 获得地图中所有节点的y虚拟偏移量
    def getVirYOffset(self) -> int:
        return self.virYOffset
    
    # 更新所有节点的虚拟坐标偏移量
    def updateNodeBias(self):
        for r in self.roads:
            # r.getStartNode().biasAbsVirCoordinates(Coordinates(self.virXOffset, self.virYOffset))
            # r.getEndNode().biasAbsVirCoordinates(Coordinates(self.virXOffset, self.virYOffset))
            nodes = r.getNodes()
            for node in nodes:
                node.biasAbsVirCoordinates(Coordinates(self.virXOffset, self.virYOffset))
    
    # 更新所有节点的绝对画框坐标
    def updateNodeDraw(self):
        baseCoo = Coordinates(self.drawWindow.width()//20, self.drawWindow.height()//20)
        for r in self.roads:
            # r.getStartNode().setAbsRelCoordinates(self.realTimes, baseCoo)
            # r.getEndNode().setAbsRelCoordinates(self.realTimes, baseCoo)
            nodes = r.getNodes()
            for node in nodes:
                node.setAbsRelCoordinates(self.realTimes, baseCoo)

    # 生成每一条路的两条线的坐标
    def genDrawLines(self): 
        for r in self.roads:
            r.gen2Lines()

    # 获得所有路
    def getRoads(self) -> List[Road]:
        return self.roads
    
    # 递归更新地图上所有汽车的位置（道路更新状态机）
    def try_update_queue(self, obj, from_direction: int, turn_direction: int, mov_pix: float, min_dis: float, mov_ang:float, min_ang:float) -> bool:
        """
        更新地图上所有汽车的位置（道路更新状态机）
        :param obj: 要更新的对象，可以是Road或Node
        :param from_direction: 车辆的来源方向
        :param turn_direction: 函数外部指定当前函数调用时，车辆的转向方向(如果是在路口转向的话，该参数可以是RIGHT，LEFT，STRAIGHT，BACK；如果在道路上，只能STRAIGHT)
        :param mov_pix: 车辆的移动距离
        :param min_dis: 车辆的最小间距
        :param mov_ang: 车辆的转向角度
        :param min_ang: 车辆的最小间距角度
        :return: 成功更新返回True，否则返回False
        """
        # TODO: debug信息
        print(obj.name)
        directs = ['N', 'E', 'S', 'W']
        turn_directs = ["BACK", "LEFT", "STRAIGHT", "RIGHT"]
        print(f" from_direction: {directs[from_direction]}, turn_direction: {turn_directs[turn_direction]}")
        last_car = None
        if type(obj) == Road:
            # 如果哪怕车辆前进最大距离都不会出界，则直接移动并返回True
            if not obj.will_move_outof_bound(from_direction, mov_pix):
                print(f"  will not move out of bound, move straight")
                _,_=obj.update_vehicles_bydirec(mov_pix, min_dis, from_direction, None)
                return True
            # 如果车辆移动的最大距离会出界，则判断最前方的车是右转，直行还是左转（当然左转是不可能的）
            else:
                # 汽车在道路上开往的方向
                to_direction = (from_direction+STRAIGHT)%4
                # 汽车在道路上开往的节点
                to_node = obj.getNode(to_direction)

                if to_node is None:
                    car, to_node = obj.update_vehicles_bydirec(mov_pix, min_dis, from_direction, None)
                    if car is not None:
                        to_node.add_car(car, from_direction)
                        return True
                # 这条道路的这个方向上的最前方的车
                front_car = obj.get_front_car(from_direction)
                # 这个车开往节点的下一个节点
                next_node = front_car.get0node()
                if next_node is None:
                    car, tonode = obj.update_vehicles_bydirec(mov_pix, min_dis, from_direction, None)
                    tonode.add_car(car, to_direction)
                    # TODO: assert 判断车辆走到尽头，还可以判断车辆的paths是不是空的
                    return True
                # 获得front_car在to_node转向的方向
                assert to_node != next_node, "to_node should not be next_node"
                turn_direction = to_node.get_turn_direction(from_direction, next_node)

                # 如果是左转，处理一下
                if turn_direction == LEFT:
                    turn_direction = to_node.switch_left2other_direction(from_direction, front_car)
                
                traffic_state = to_node.get_traffic_state(from_direction)
                # 道路上下一步是直行
                if turn_direction == STRAIGHT:
                    if traffic_state == STOP:
                        # 红灯，需要走到马路边界
                        car, tonode = obj.update_vehicles_bydirec(mov_pix, min_dis, from_direction, None, True)
                        assert car is None , "waiting for red light, car should be None"
                        assert tonode is None , "waiting for red light, tonode should be None"
                        return True
                    
                    # 绿灯，可以直行，但是需要先看看路口这个直行道是否能走
                    # 这个变量是获取to_node这个节点中，obj路线前面的车辆队列是否更新车辆位置状态，更新则为True，否则为False
                    update_state = to_node.get_straight_car_queue_updated(from_direction)
                    if not update_state:
                        # 路口车辆位置没更新，需要更新一下子。
                        success_update = self.try_update_queue(to_node, from_direction, turn_direction, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            return False
                    last_car = to_node.get_straight_end_car(from_direction)

                # 道路上头车下一步是右转    
                elif turn_direction == RIGHT:
                    update_state = to_node.get_right_car_queue_updated(from_direction)
                    # 如果to_node的右转车道没更新，那么更新一下子。
                    if not update_state:
                        success_update = self.try_update_queue(to_node, from_direction, turn_direction, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            return False
                    last_car = to_node.get_right_end_car(from_direction)
                
                # 道路上头车下一步是掉头
                elif turn_direction == BACK:
                    update_state = to_node.get_back_car_queue_updated(from_direction)
                    if not update_state:
                        success_update = self.try_update_queue(to_node, from_direction, turn_direction, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            return False
                    last_car = to_node.get_back_end_car(from_direction)

                elif turn_direction == LEFT:
                    raise ValueError("turn_direction should not be LEFT")
                else:
                    raise ValueError("turn_direction should be STRAIGHT, RIGHT, LEFT, BACK")
                
                car, tonode = obj.update_vehicles_bydirec(mov_pix, min_dis, from_direction, last_car)
                # assert car is not None, "现在在更新道路上的车辆，前面的判断中已经判断出当次更新会导致车辆溢出了，但是没溢出，不正常"
                # assert tonode is not None, "tonode should not be None"
                if car is not None:
                    tonode.add_car(car, from_direction)
                return True

        elif type(obj) == Node:
            if turn_direction == STRAIGHT:
                to_direction = (from_direction+STRAIGHT)%4
                # 直走应该判断前面的右转是不是更新了，
                new_from_direction = (to_direction+4-RIGHT)%4
                # print("  straight need right")
                update_state = obj.get_right_car_queue_updated(new_from_direction)
                if not update_state:
                    # print(f"   before right, {new_from_direction}, {turn_directs[RIGHT]}")
                    success_update = self.try_update_queue(obj, new_from_direction, RIGHT, mov_pix, min_dis, mov_ang, min_ang)
                    # print("   after right")
                    if not success_update:
                        return False

                # 前路
                to_road: Road = obj.get_path(to_direction)
                car, to_road = obj.update_straight_cars_position(mov_pix, min_dis, from_direction, last_car)
                if car is not None:
                    to_road.add_car(car, min_dis, from_direction)
                return True
            
            elif turn_direction == RIGHT:
                to_direction = (from_direction+RIGHT)%4
                # 前路
                to_road = obj.get_path(to_direction)
                # 前路的来时方向
                new_from_direction = (to_direction+4-STRAIGHT)%4
                # 看看前路是否更新过
                print(f"    will update {obj.name}, from_direction:{directs[from_direction]}, new_from_direction: {directs[new_from_direction]}")
                if to_road is not None:
                    update_state = to_road.get_car_queue_updated(new_from_direction)
                    # 前路没更新过，先更新一下
                    if not update_state:
                        success_update = self.try_update_queue(to_road, new_from_direction, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            return False
                # 更新后，更新当前节点的右转车道
                car, to_road = obj.update_right_cars_position(mov_ang, min_ang, from_direction, last_car)
                if car is not None:
                    to_road.add_car(car, min_dis, new_from_direction)
                return True

            elif turn_direction == BACK:
                # 掉头需要先判断对面的直行道是不是更新了
                # 然后再判断对面直行车道是否有车辆
                # 找到将要走向的方向。其实还是来时的方向
                to_direction = (from_direction+BACK)%4
                new_from_direction = (to_direction+4-STRAIGHT)%4
                # 1. 看看前路是否更新过
                update_state = obj.get_straight_car_queue_updated(new_from_direction)
                # 2. 前路没更新过，先更新一下
                if not update_state:
                    success_update = self.try_update_queue(obj, new_from_direction, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                    if not success_update:
                        return False
                # 3. 前路更新过，更新掉头车道
                if obj.get_straight_car_queue_length(new_from_direction) == 0:
                    # 这种情况下对面直行车道没有车辆，我们可以开始更新掉头车道里车辆的位置了。
                    car, to_road = obj.update_back_cars_position(mov_pix, min_dis, from_direction)
                    # 4. 车辆溢出，将车辆加入新的道路
                    if car is not None:
                        to_road.add_car(car, min_dis, new_from_direction)
                    return True
                else:
                    obj.mark_back_car_queue_updated(from_direction)
                    return True
        else:
            raise ValueError("type(obj)= ", type(obj), "obj should be Road or Node")

    # 更新地图上所有汽车的位置
    def updateAllCarsPosition(self, mov_pix:float, min_dis: float, mov_ang: float, min_ang: float):
        # 首先更新map中所有道路上汽车的位置
        car_containers = []
        for r in self.roads:
            car_containers.append(r)
        for node in self.genCarNodes:
            car_containers.append(node)
        for node in self.trafficLightNodes:
            car_containers.append(node)
        
        while len(car_containers) != 0:
            obj = car_containers[0]
            print(obj.name)
            
            if type(obj)==Road:
                road_direction = obj.get_direction()
                if road_direction == NSD:
                    Nupdate = obj.get_car_queue_updated(N)
                    Supdate = obj.get_car_queue_updated(S)
                    if not Nupdate:
                        self.try_update_queue(obj, N, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                    elif not Supdate:
                        self.try_update_queue(obj, S, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                elif road_direction == WED:
                    Wupdate = obj.get_car_queue_updated(W)
                    Eupdate = obj.get_car_queue_updated(E)
                    if not Wupdate:
                        self.try_update_queue(obj, W, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                    if not Eupdate:
                        self.try_update_queue(obj, E, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
            elif type(obj)==Node:
                print(obj.get_all_updated())
                updated = False
                for from_direction in [W,E]:
                    if updated:
                        break
                    if not obj.get_back_car_queue_updated(from_direction):
                        updated = True
                        success_update = self.try_update_queue(obj, from_direction, BACK, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            raise ValueError("update back car failed")
                for from_direction in [N,E,S,W]:
                    if updated:
                        break
                    if not obj.get_straight_car_queue_updated(from_direction):
                        updated = True
                        success_update = self.try_update_queue(obj, from_direction, STRAIGHT, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            raise ValueError("update straight car failed")
                for from_direction in [N,E,S,W]:
                    if updated:
                        break
                    if not obj.get_right_car_queue_updated(from_direction):
                        updated = True
                        success_update = self.try_update_queue(obj, from_direction, RIGHT, mov_pix, min_dis, mov_ang, min_ang)
                        if not success_update:
                            raise ValueError("update right car failed")
            print(f"===================len={len(car_containers)}===========================")
            all_not_updated = False
            while not all_not_updated:
                all_not_updated = True
                for i,obj in enumerate(car_containers):
                    if obj.get_all_updated():
                        car_containers.pop(i)
                        all_not_updated = False
                        break
        # 更新结束后，清空所有路线和路段的更新状态
        for r in self.roads:
            r.clean_all_update()
        for node in self.genCarNodes:
            node.clean_all_update()
        for node in self.trafficLightNodes:
            node.clean_all_update()


    def genNewCars(self, min_dis: float):
        # 其次生成新的车辆。
        for node in self.genCarNodes:
            # 生成车辆
            car, direction = node.genCar()
            if car is None:
                continue
            car.add_start_node(node)
            end_node = node
            while end_node == node:
                rindex = random.randint(0,len(self.genCarNodes)-1)
                for tmpindex, tmpnode in enumerate(self.genCarNodes):
                    if tmpindex == rindex:
                        end_node=tmpnode
            print(f"gen car begin:{node.name}, end:{end_node.name}")
            print(f"  paths:{self.paths[node][end_node]}")
            car.add_paths(copy.copy(self.paths[node][end_node]))
            print("after add paths")
            car.pop0node()
            car.pop0node()
            # 获得路口对应的道路
            r: Road = node.paths[direction]
            # 将汽车加入到道路
            # print("add car")
            from_direction = (direction+4-STRAIGHT)%4
            r.add_car(car, min_dis, from_direction)

    # def genNewCars1(self, min_dis: float):


    # 更新地图中所有交通路口的所有的的交通信号灯
    def updateAllTrafficLight(self):
        for node in self.trafficLightNodes:
            node.update_trafficlight()
